//Header files of the libraries that will be used
#include <assert.h>
#include <stdio.h>
#include <string.h>
#include <math.h>

#include "controller_interface.h"

#include <bot_core/bot_core.h>
#include <bot_param/param_client.h>
#include <lcm/lcm.h>
#include <lcmtypes/quad_lcmtypes.h>
#include <lcmtypes/rigid_body_pose_t.h>

#include <quad_common/carmen3d_2d_hacks.h>

#include <lcmtypes/lcmtypes.h>
#include <lcmtypes/mission_executor_defines.hpp>
#include <lcmtypes/asctec_sdk_driver.h>

#include <getopt.h>

#include <deque>

#include <ConciseArgs>

using namespace std;

static lcm_t * lcm;
static BotParam * bot_param;

#define LAND_RAMP_DOWN_THRESH -.6

//globals:
static bool simulator = false;
static float simulation_rate = 100;
static int delay_amount = -1;
static deque<bot_core_pose_t*> delay_queue;

static int64_t curr_utime = 0;
static quad_waypoint_t previous_goal;
static quad_waypoint_t current_goal;
static quad_waypoint_t current_set_point;
static quad_waypoint_t globalState;  //store the state in a waypt to make life easier :-/
//static carmen3d_system_vitals_msg_t *vitals = NULL;

static asctec_sdk_driver_quad_control_msg_t command_msg;

static int goalReached = 1;
static int newGoal = 0;

static double pBiasEMA = 0;
static double rBiasEMA = 0;
static double zBiasEMA = 0;
static double yawBiasEMA = 0;

static double pIntegral = 0;
static double rIntegral = 0;
static double zIntegral = 0;
static double yawIntegral = 0;

//parameters to be read from param server
std::string robot_name;
bool param_tune = false;

static double lqr_K[4 * 7] = { 0.0 };
static double control_bias[4] = { 0.0 };
static double battery_gain;

static int pdControl;  //use these pd gains instead of lqr gains
static double Kp_pr, Kp_z, Kp_yaw;
static double Kd_pr, Kd_z, Kd_yaw;
static double Ki_pr, Ki_z, Ki_yaw;

static double prBiasEMA_alpha;
static double zBiasEMA_alpha;
static double yawBiasEMA_alpha;

static double pr_relativeClamp;
static double pr_absoluteClamp;
static double thrust_clamp_max;
static double thrust_clamp_min;
static double thrust_relativeClamp;
static double yaw_clamp;
static double thrust_slewRateLimit;

static waypoint_params_t waypt_params;

static int useSetPointVelocity;
static double max_accel_xy;
static double max_accel_z;

static int yaw_first;

//TODO: Better way to handle channel assignment.
static const char * pose_channel = NULL;
static bool use_mav_estimator;

static void publish_controller_status_observation(
    mission_executor::Observation obs) {
  mission_executor_observation_t mission_executor_observation;
  mission_executor_observation.observation = obs;
  mission_executor_observation_t_publish(lcm, CONTROLLER_STATUS_CHANNEL,
                                             &mission_executor_observation);

}

static void publish_controls(
    asctec_sdk_driver_quad_control_msg_t * command_msg) {
  if (1) {
    static int64_t last_send_command_time = -1;
    if (curr_utime - last_send_command_time > 2e6) {
      fprintf(
          stderr,
          "CONTROL: (%+0.3f,%+0.3f,%+0.3f) %+0.3f\tBIAS: (%+0.3f,%+0.3f,%+0.3f) %+0.3f\tGoal: (%+.2f,%+.2f,%+.2f) %+.2f\tPos: (%+.2f,%+.2f,%+.2f) %+.2f\tInt: (%+.2f,%+.2f,%+.2f) %+.2f\tTleft=%.3f\tPD:%d KPPR:%.2f\n",
          command_msg->roll, command_msg->pitch, command_msg->thrust,
          command_msg->yaw, command_msg->pitch_bias, command_msg->roll_bias,
          command_msg->thrust_bias, command_msg->yaw_bias,
          current_set_point.xyzt[0], current_set_point.xyzt[1],
          current_set_point.xyzt[2], current_set_point.xyzt[3],
          globalState.xyzt[0], globalState.xyzt[1], globalState.xyzt[2],
          globalState.xyzt[3], command_msg->pitch_int, command_msg->roll_int,
          command_msg->thrust_int, command_msg->yaw_int,
          current_set_point.utime / 1.0e6, pdControl, Kp_pr);
      last_send_command_time = curr_utime;
    }
  }
//  if (!simulator)  //don't publish if we're in simulator mode
  asctec_sdk_driver_quad_control_msg_t_publish(lcm, "CONTROL_COMMAND",
                                                      command_msg);  
  command_msg->utime = curr_utime;
}

/** Compute Feedback: Computes control outputs from LQR gains
 *
 * input: bodyState, bodyGoal, command_msg
 *
 * modifies: command_msg
 */
static void computeFeedback(
    quad_waypoint_t * bodyState, quad_waypoint_t * bodyGoal,
    asctec_sdk_driver_quad_control_msg_t * command_msg) {
  static double prevThrust = 0;
  // x = 7-state (4 pos, 3 vel) error between current body & goal body
  double x[7] = { 0 };
  x[0] = bodyState->xyzt[0] - bodyGoal->xyzt[0];
  x[1] = bodyState->xyzt[1] - bodyGoal->xyzt[1];
  x[2] = bodyState->xyzt[2] - bodyGoal->xyzt[2];
  x[3] = bot_mod2pi(bodyState->xyzt[3] - bodyGoal->xyzt[3]);

  if (useSetPointVelocity) {
    x[4] = bodyState->xyzt_dot[0] - bodyGoal->xyzt_dot[0];
    x[5] = bodyState->xyzt_dot[1] - bodyGoal->xyzt_dot[1];
    x[6] = bodyState->xyzt_dot[2] - bodyGoal->xyzt_dot[2];
  } else {
    x[4] = bodyState->xyzt_dot[0];
    x[5] = bodyState->xyzt_dot[1];
    x[6] = bodyState->xyzt_dot[2];
  }
  double u[4] = { 0.0 };

  // --- Compute Controls from PD or LQR Gains ---
  if (!pdControl) {
    int i, j;
    for (i = 0; i < 4; i++) {  //compute u = -Kx;
      for (j = 0; j < 7; j++)
        u[i] -= lqr_K[7 * i + j] * x[j];
    }
  } else {  //just use the pd gains
    u[0] = -(Kp_pr * x[0] + Kd_pr * x[4]);
    u[1] = -(Kp_pr * x[1] + Kd_pr * x[5]);
    u[2] = -(Kp_z * x[2] + Kd_z * x[6]);
    u[3] = -(Kp_yaw * x[3]);  //yaw is P control only
  }

  //check whether we're on the ground:
  static int onGround = true;
  if (bodyGoal->waypt_type == QUAD_WAYPOINT_T_TYPE_LAND) {
    if (bodyGoal->xyzt[2] - bodyState->xyzt[2] < LAND_RAMP_DOWN_THRESH
        && fabs(bodyState->xyzt_dot[2]) < .2) {
      if (!onGround) {
        fprintf(stderr, "on ground!\n");
        onGround = true;
      }
    }
  } else {
    if (onGround) {
      fprintf(stderr, "starting flight!\n");
      onGround = false;
    }
  }
  //  fprintf(stderr,"waypt_type is %d, onground is %d, bod_vel = %+2f delta ` %f\n",bodyGoal->waypt_type,onGround,bodyState->xyzt_dot[2], bodyGoal->xyzt[2] - bodyState->xyzt[2]);

  // --- Integral Term ---
  if (onGround) {
    //zeroed if on ground
    pIntegral = 0;
    rIntegral = 0;
    zIntegral = 0;
    yawIntegral = 0;
  } else {
    if (bot_sgn(x[0]) == bot_sgn(bodyState->xyzt_dot[0]))  //we're not moving towards the goal
      pIntegral += x[0];
    if (bot_sgn(x[1]) == bot_sgn(bodyState->xyzt_dot[1]))  //we're not moving towards the goal
      rIntegral += x[1];
    if (bot_sgn(x[2]) == bot_sgn(bodyState->xyzt_dot[2]))  //we're not moving towards the goal
      zIntegral += x[2];
    if (bot_sgn(x[3]) == bot_sgn(bodyState->xyzt_dot[3]))  //we're not moving towards the goal
      yawIntegral += x[3];
  }
  command_msg->pitch_int = pIntegral;
  command_msg->roll_int = rIntegral;
  command_msg->thrust_int = zIntegral;
  command_msg->yaw_int = yawIntegral;

  // --- Complete PID Control Command ---
  //TODO:put in caps on integral term!
  command_msg->pitch = u[0] + pBiasEMA - Ki_pr * pIntegral;
  command_msg->roll = u[1] + rBiasEMA - Ki_pr * rIntegral;
  command_msg->thrust = u[2] + zBiasEMA - Ki_z * zIntegral;
  command_msg->yaw = u[3] + yawBiasEMA - Ki_yaw * yawIntegral;

  // --- Clamping ---
  //clamp around "neutral"
  if (bodyGoal->waypt_type != QUAD_WAYPOINT_T_TYPE_TAKEOFF || goalReached) {  //don't use relative clamp while we're taking off... to allow things to settle
    command_msg->pitch =
        bot_clamp(command_msg->pitch, -pr_relativeClamp + pBiasEMA, pr_relativeClamp + pBiasEMA);
    command_msg->roll =
        bot_clamp(command_msg->roll, -pr_relativeClamp + rBiasEMA, pr_relativeClamp + rBiasEMA);
  }

  //  if (bodyGoal->waypt_type != QUAD_WAYPOINT_T_TYPE_LAND) {
  //    command_msg->thrust = carmen_clamp(-thrust_relativeClamp + zBiasEMA, command_msg->thrust,
  //        thrust_relativeClamp + zBiasEMA);
  //  }

  //absolute clamp
  command_msg->pitch =
      bot_clamp(command_msg->pitch,-pr_absoluteClamp, pr_absoluteClamp);
  command_msg->roll =
      bot_clamp(command_msg->roll,-pr_absoluteClamp, pr_absoluteClamp);

  command_msg->thrust =
      bot_clamp( command_msg->thrust,thrust_clamp_min,thrust_clamp_max);
  command_msg->yaw = bot_clamp(command_msg->yaw,-yaw_clamp, yaw_clamp);

  // --- Handle BIAS ---
  //put the current biases into the message
  command_msg->pitch_bias = pBiasEMA;
  command_msg->roll_bias = rBiasEMA;
  command_msg->thrust_bias = zBiasEMA;
  command_msg->yaw_bias = yawBiasEMA;

  //update the ema if we're not on the ground
  if (bodyGoal->xyzt[2] > 0) {
    if (fabs(bodyState->xyzt_dot[0]) < .3 || goalReached) {  //and not moving fast
      pBiasEMA = prBiasEMA_alpha * command_msg->pitch
          + (1 - prBiasEMA_alpha) * pBiasEMA;
    }
    if (fabs(bodyState->xyzt_dot[1]) < .3 || goalReached) {  //and not moving fast
      rBiasEMA = prBiasEMA_alpha * command_msg->roll
          + (1 - prBiasEMA_alpha) * rBiasEMA;
    }
    if (fabs(bodyState->xyzt_dot[2]) < .1 || goalReached) {  // and not moving fast
      zBiasEMA = zBiasEMA_alpha * command_msg->thrust
          + (1 - zBiasEMA_alpha) * zBiasEMA;
    }
    yawBiasEMA = yawBiasEMA_alpha * command_msg->yaw
        + (1 - yawBiasEMA_alpha) * yawBiasEMA;
  }

  // --- Handle Ramping Down if LANDING ---
  static double rampDown = 0;
  static int rampDownStatus = 0;  // 0: on ground and done ramping; -1: flying; 1: on ground and ramping down
  if (onGround) {
    if (rampDownStatus == -1) {
      rampDown = command_msg->thrust;
      rampDownStatus = 1;
      fprintf(stderr, "On Ground... Starting Rampdown\n");
    }
    if (rampDownStatus > 0)
      rampDown -= .005;
    if (rampDown <= 0 && rampDownStatus > 0) {
      rampDown = 0;
      rampDownStatus = 0;
      fprintf(stderr, "Ramp down done\n");
    }
    //we should be sitting on the ground...
    command_msg->pitch = 0;
    command_msg->roll = 0;
    command_msg->thrust = rampDown;
    command_msg->yaw = 0;
  } else {
    rampDownStatus = -1;
  }

  // --- Limit Thrust SlewRate ---
  if (fabs(command_msg->thrust - prevThrust > thrust_slewRateLimit)) {
    fprintf(stderr, "slew rate limit hit for thrust, (%f - %f)\n",
            command_msg->thrust, prevThrust);
    int changeSign = command_msg->thrust - prevThrust > 0;
    command_msg->thrust = prevThrust + changeSign * thrust_slewRateLimit;
  }

  prevThrust = command_msg->thrust;
}

static double computeCurrVelocity(double startPos, double endPos,
                                  double currPos, double currVel, double dt,
                                  double maxVel, double acc) {
  double newVel = 0;
  double stopTime = currVel / acc;
  double stopDist = currVel * stopTime - .5 * acc * acc;  //distance needed to stop
  double distToGoal = fabs(endPos - currPos);
  double halfwayDist = fabs(endPos - startPos) / 2;
  if (distToGoal < stopDist + .5) {
    acc = -acc;  //we should be slowing down
  }
  newVel = currVel + dt * acc;
  newVel = bot_clamp(newVel,0.2, maxVel);
  //  fprintf(stderr,"distToGoal = %f, stopDist=%f acc = %+.3f vel=%f\n",distToGoal,stopDist,acc,newVel);
  return newVel;
}

/**
 * Compute Current Goal:
 * linearly interpolating the waypoint subject to acceleration constraints
 *
 * input: prev, goal, current
 *
 * modifies: goal, current
 */
static void computeSetPoint(quad_waypoint_t * prev_goal, quad_waypoint_t * goal,
                            quad_waypoint_t * setPoint) {
  //make sure we're not dividing by zero
  goal->xyzt_dot[0] = fmax(fabs(goal->xyzt_dot[0]), .01);
  goal->xyzt_dot[1] = fmax(fabs(goal->xyzt_dot[1]), .01);
  goal->xyzt_dot[2] = fmax(fabs(goal->xyzt_dot[2]), .01);
  goal->xyzt_dot[3] = fmax(fabs(goal->xyzt_dot[3]), .01);

  double dt = (curr_utime - setPoint->utime) * 1e-6;  //time since last waypoint update
  setPoint->utime = curr_utime;
  int reachedXY = 0, reachedYaw = 0, reachedZ = 0;

  //compute Yaw first
  if (fabs(bot_mod2pi(goal->xyzt[3] - setPoint->xyzt[3])) < bot_to_radians(1)) {
    reachedYaw = 1;
    setPoint->xyzt[3] = goal->xyzt[3];
    setPoint->xyzt_dot[3] = 0;  //always set to 0
  } else {
    int yawDir = bot_sgn(bot_mod2pi(goal->xyzt[3] - setPoint->xyzt[3]));
    setPoint->xyzt[3] += dt * goal->xyzt_dot[3] * yawDir;
    setPoint->xyzt_dot[3] = 0;  //always set to 0
  }

  if (!yaw_first || reachedYaw) {
    //compute Z
    if (fabs(setPoint->xyzt[2] - goal->xyzt[2]) < .05
        || fabs(setPoint->xyzt[2] - prev_goal->xyzt[2])
            > fabs(goal->xyzt[2] - prev_goal->xyzt[2])) {
      reachedZ = 1;
      setPoint->xyzt[2] = goal->xyzt[2];
      setPoint->xyzt_dot[2] = 0;
    } else {
      setPoint->xyzt_dot[2] = computeCurrVelocity(prev_goal->xyzt[2],
                                                  goal->xyzt[2],
                                                  setPoint->xyzt[2],
                                                  setPoint->xyzt_dot[2], dt,
                                                  goal->xyzt_dot[2],
                                                  max_accel_z);
      int zDir = bot_sgn(goal->xyzt[2] - setPoint->xyzt[2]);
      setPoint->xyzt[2] += zDir * setPoint->xyzt_dot[2] * dt;
    }

    //compute XY
    double goalXY[2] = { goal->xyzt[0], goal->xyzt[1] };
    double currXY[2] = { setPoint->xyzt[0], setPoint->xyzt[1] };
    double prevXY[2] = { prev_goal->xyzt[0], prev_goal->xyzt[1] };
    double totalDist = bot_vector_dist_2d(goalXY, prevXY);
    double distToGoal = bot_vector_dist_2d(goalXY, currXY);
    double distToStart = bot_vector_dist_2d(prevXY, currXY);
    if (distToGoal < .05 || distToStart > totalDist) {
      reachedXY = 1;
      setPoint->xyzt[0] = goal->xyzt[0];
      setPoint->xyzt[1] = goal->xyzt[1];
      setPoint->xyzt_dot[0] = 0;
      setPoint->xyzt_dot[1] = 0;
    } else {
      double maxVel = fmin(goal->xyzt_dot[0], goal->xyzt_dot[1]);
      double curVel[2] = { setPoint->xyzt_dot[0], setPoint->xyzt_dot[1] };
      double vel = bot_vector_magnitude_2d(curVel);
      vel = computeCurrVelocity(0, totalDist, distToStart, vel, dt, maxVel,
                                max_accel_xy);
      bot_vector_subtract_2d(goalXY, currXY, curVel);
      bot_vector_normalize_2d(curVel);
      bot_vector_scale_2d(curVel, vel);

      setPoint->xyzt[0] += curVel[0] * dt;
      setPoint->xyzt[1] += curVel[1] * dt;
      setPoint->xyzt_dot[0] = curVel[0];
      setPoint->xyzt_dot[1] = curVel[1];
    }
  }

  if (reachedXY && reachedYaw && reachedZ) {
    if (!goalReached) {
      publish_controller_status_observation(
          mission_executor::TRAJECTORY_COMPLETE);
    }
    goalReached = 1;
  }

}

/**
 * State Handler = MAIN (Effectively the "main" function in this program)
 * Function that is called if it gets a pose message.
 */
static void pose_handler(const lcm_recv_buf_t *rbuf __attribute__((unused)),
                         const char * channel __attribute__((unused)),
                         const bot_core_pose_t * msg,
                         void * user __attribute__((unused))) {
  static bot_core_pose_t * cur_pose_msg = NULL;
  if (delay_amount <= 0) {
    if (cur_pose_msg != NULL)
      bot_core_pose_t_destroy(cur_pose_msg);
    cur_pose_msg = bot_core_pose_t_copy(msg);
  } else {
    //put current message into queue so it gets delayed...
    bot_core_pose_t * tmp = bot_core_pose_t_copy(msg);
    delay_queue.push_back(tmp);
    while ((int) delay_queue.size() > delay_amount) {
      if (cur_pose_msg != NULL)
        bot_core_pose_t_destroy(cur_pose_msg);
      cur_pose_msg = delay_queue.front();
      delay_queue.pop_front();
    }
  }

  //TODO: is there a cleaner/better way to do this?
  //    if (!check_pose_is_valid(cur_pose_msg, NULL)) {
  globalState.xyzt[0] = cur_pose_msg->pos[0];
  globalState.xyzt[1] = cur_pose_msg->pos[1];
  globalState.xyzt[2] = cur_pose_msg->pos[2];

  globalState.xyzt_dot[0] = cur_pose_msg->vel[0];
  globalState.xyzt_dot[1] = cur_pose_msg->vel[1];
  globalState.xyzt_dot[2] = cur_pose_msg->vel[2];

  double rpy[3];
  bot_quat_to_roll_pitch_yaw(cur_pose_msg->orientation, rpy);
  globalState.xyzt[3] = rpy[2];
  globalState.utime = cur_pose_msg->utime;
  //  }
  curr_utime = cur_pose_msg->utime;

  command_msg.utime = curr_utime;

  //  char buf[128];
  //  sprintf(buf, "%s control %.7f", globalState.host, curTime);
  //  command_msg.host = buf;

  //compute the Body_State
  quad_waypoint_t bodyState = globalState;
  carmen3d_2d_hacks_vec_global_to_body(globalState.xyzt, globalState.xyzt[3],
                                       bodyState.xyzt);
  carmen3d_2d_hacks_vec_global_to_body(globalState.xyzt_dot,
                                       globalState.xyzt[3], bodyState.xyzt_dot);

  computeSetPoint(&previous_goal, &current_goal, &current_set_point);
  quad_waypoint_t bodyGoal = current_set_point;
  carmen3d_2d_hacks_vec_global_to_body(current_set_point.xyzt,
                                       globalState.xyzt[3], bodyGoal.xyzt);
  carmen3d_2d_hacks_vec_global_to_body(current_set_point.xyzt_dot,
                                       globalState.xyzt[3], bodyGoal.xyzt_dot);

  bot_tictoc("LQR");
  computeFeedback(&bodyState, &bodyGoal, &command_msg);  //compute simple feedback without simulating forward
  bot_tictoc("LQR");

  command_msg.utime = curr_utime;
  publish_controls(&command_msg);

  quad_waypoint_t_publish(lcm, WAYPOINT_CURR_CHANNEL, &current_set_point);  //send out the current waypt for plotting and display :-)
  newGoal = 0;
}

/**
 * TODO: remove this function...
 */
static void mav_state_estimator_pose_handler(
    const lcm_recv_buf_t *rbuf __attribute__((unused)),
    const char * channel __attribute__((unused)), const rigid_body_pose_t * msg,
    void * user __attribute__((unused))) {
  // Have to transform velocity from body frame to global frame.
  double body_vel[3];
  memcpy(body_vel, msg->vel, 3 * sizeof(double));
  bot_core_pose_t * gmsg = bot_core_pose_t_copy((const bot_core_pose_t *) msg);

  bot_quat_rotate_to(gmsg->orientation, body_vel, gmsg->vel);
  pose_handler(rbuf, channel, gmsg, user);
  bot_core_pose_t_destroy(gmsg);
}

static void waypoint_handler(const lcm_recv_buf_t *rbuf __attribute__((unused)),
                             const char * channel __attribute__((unused)),
                             const quad_waypoint_t * new_waypt,
                             void * user __attribute__((unused))) {
  newGoal = carmen3d_controller_update_waypoints(&waypt_params, lcm, new_waypt,
                                                 &previous_goal,
                                                 &current_set_point,
                                                 &current_goal, &globalState,
                                                 goalReached, curr_utime);

  if (newGoal) {
    goalReached = 0;
    publish_controller_status_observation(
        mission_executor::TRAJECTORY_RECEIVED);
  }
}

static void mission_executor_trajectory_handler(
    const lcm_recv_buf_t *rbuf __attribute__((unused)),
    const char * channel __attribute__((unused)),
    const mission_executor_trajectory_t * trajectory,
    void * user __attribute__((unused))) {
  switch (trajectory->mode) {
    case mission_executor::TAKEOFF:
      carmen3d_controller_publish_takeoff_waypoint(lcm, bot_timestamp_now());
      break;
    case mission_executor::LAND:
    case mission_executor::FAILSAFE_LANDING:
      carmen3d_controller_publish_land_waypoint(lcm, bot_timestamp_now());
      break;
    case mission_executor::HOLD_POSITION:
      carmen3d_controller_publish_stop_waypoint(lcm, bot_timestamp_now());
      break;

    case mission_executor::CRUISE:
      quad_waypoint_t waypt_msg;
      carmen3d_controller_set_noop(&waypt_msg);
      waypt_msg.waypt_type = QUAD_WAYPOINT_T_TYPE_WAYPT;
      for (int i = 0; i < 3; i++)
        waypt_msg.xyzt[i] = trajectory->end[i];

      waypt_msg.xyzt_dot[0] = trajectory->velocity;
      waypt_msg.xyzt_dot[1] = trajectory->velocity;

      waypt_msg.nonce = random();
      waypt_msg.utime = bot_timestamp_now();
      waypt_msg.sender = QUAD_WAYPOINT_T_SENDER_MISSION_PLANNER;

      carmen3d_controller_sanity_check_and_publish(lcm,
                                                   WAYPOINT_COMMAND_CHANNEL,
                                                   &waypt_msg);
      break;
    default:
      fprintf(stderr, "ERROR: invalid mission executor trajectory type: %d\n",
              trajectory->mode);
      assert(false);
      break;

  }

}

static void sim_teleport_handler(
    const lcm_recv_buf_t *rbuf __attribute__((unused)),
    const char * channel __attribute__((unused)), const quad_waypoint_t * msg,
    void * user __attribute__((unused))) {
  previous_goal = *msg;
  current_set_point = *msg;
  current_goal = *msg;
  fprintf(stderr, "Teleporting to : %f, %f, %f\n", msg->xyzt[0], msg->xyzt[1],
          msg->xyzt[2]);
}

static gboolean simulatorPosePublish(void * user_data) {
  //publish a pose message at current_set_point...

  bot_core_pose_t pose;
  memset(&pose, 0, sizeof(pose));
  pose.pos[0] = current_set_point.xyzt[0];
  pose.pos[1] = current_set_point.xyzt[1];
  pose.pos[2] = current_set_point.xyzt[2];
  double rpy[3] = { 0, 0, current_set_point.xyzt[3] };
  bot_roll_pitch_yaw_to_quat(rpy, pose.orientation);

  pose.utime = bot_timestamp_now();

  bot_core_pose_t_publish(lcm, pose_channel, &pose);

  //publish body to local frame
  bot_core_rigid_transform_t body_to_local;
  memset(&body_to_local, 0, sizeof(body_to_local));
  body_to_local.trans[0] = current_set_point.xyzt[0];
  body_to_local.trans[1] = current_set_point.xyzt[1];
  body_to_local.trans[2] = current_set_point.xyzt[2];
  bot_roll_pitch_yaw_to_quat(rpy, body_to_local.quat);
  body_to_local.utime = bot_timestamp_now();
  bot_core_rigid_transform_t_publish(lcm, "BODY_TO_LOCAL", &body_to_local);

  return TRUE;
}

static void param_update_handler(BotParam * old_param, BotParam * param,
                                 int64_t utime, void *user) {


  //vehicle specific params
  char paramStr[512];
  //specify whether to use the PID controll parameters or the lqr ones
  sprintf(paramStr, "controller.%s.pdControl", robot_name.c_str());
  pdControl = bot_param_get_int_or_fail(bot_param, paramStr);

  sprintf(paramStr, "controller.%s.Kp_pr", robot_name.c_str());
  Kp_pr = bot_param_get_double_or_fail(bot_param, paramStr);
  sprintf(paramStr, "controller.%s.Kp_z", robot_name.c_str());
  Kp_z = bot_param_get_double_or_fail(bot_param, paramStr);
  sprintf(paramStr, "controller.%s.Kp_yaw", robot_name.c_str());
  Kp_yaw = bot_param_get_double_or_fail(bot_param, paramStr);
  sprintf(paramStr, "controller.%s.Kd_pr", robot_name.c_str());
  Kd_pr = bot_param_get_double_or_fail(bot_param, paramStr);
  sprintf(paramStr, "controller.%s.Kd_z", robot_name.c_str());
  Kd_z = bot_param_get_double_or_fail(bot_param, paramStr);
  sprintf(paramStr, "controller.%s.Kd_yaw", robot_name.c_str());
  Kd_yaw = bot_param_get_double_or_fail(bot_param, paramStr);
  sprintf(paramStr, "controller.%s.Ki_pr", robot_name.c_str());
  Ki_pr = bot_param_get_double_or_fail(bot_param, paramStr);
  sprintf(paramStr, "controller.%s.Ki_z", robot_name.c_str());
  Ki_z = bot_param_get_double_or_fail(bot_param, paramStr);
  sprintf(paramStr, "controller.%s.Ki_yaw", robot_name.c_str());
  Ki_yaw = bot_param_get_double_or_fail(bot_param, paramStr);

  sprintf(paramStr, "controller.%s.battery_gain", robot_name.c_str());
  battery_gain = bot_param_get_double_or_fail(bot_param, paramStr);

  sprintf(paramStr, "controller.%s.control_bias", robot_name.c_str());
  bot_param_get_double_array_or_fail(bot_param, paramStr, control_bias, 4);  //4 vector
  pBiasEMA = control_bias[0];
  rBiasEMA = control_bias[1];
  zBiasEMA = control_bias[2];
  yawBiasEMA = control_bias[3];

  sprintf(paramStr, "controller.%s.lqr_K", robot_name.c_str());
  bot_param_get_double_array_or_fail(bot_param, paramStr, lqr_K, 7 * 4);  //7x4 matrix

  //params for adjusting biases online
  //not used anymore...
  prBiasEMA_alpha = bot_param_get_double_or_fail(bot_param,
                                                 "controller.prBiasEMA_alpha");
  zBiasEMA_alpha = bot_param_get_double_or_fail(bot_param,
                                                "controller.zBiasEMA_alpha");
  yawBiasEMA_alpha = bot_param_get_double_or_fail(
      bot_param, "controller.yawBiasEMA_alpha");

  //clamp values for controller
  pr_relativeClamp = bot_param_get_double_or_fail(
      bot_param, "controller.pr_relativeClamp");
  pr_absoluteClamp = bot_param_get_double_or_fail(
      bot_param, "controller.pr_absoluteClamp");
  thrust_clamp_min = bot_param_get_double_or_fail(
      bot_param, "controller.thrust_clamp_min");
  thrust_clamp_max = bot_param_get_double_or_fail(
      bot_param, "controller.thrust_clamp_max");
  thrust_relativeClamp = bot_param_get_double_or_fail(
      bot_param, "controller.thrust_relativeClamp");
  yaw_clamp = bot_param_get_double_or_fail(bot_param, "controller.yaw_clamp");
  thrust_slewRateLimit = bot_param_get_double_or_fail(
      bot_param, "controller.thrust_slewRateLimit");

  //waypoint params
  waypt_params.default_height = bot_param_get_double_or_fail(
      bot_param, "controller.default_height");
  waypt_params.default_yaw = bot_param_get_double_or_fail(
      bot_param, "controller.default_yaw");
  waypt_params.default_xy_vel = bot_param_get_double_or_fail(
      bot_param, "controller.default_xy_vel");
  waypt_params.default_z_vel = bot_param_get_double_or_fail(
      bot_param, "controller.default_z_vel");
  waypt_params.default_yaw_vel = bot_param_get_double_or_fail(
      bot_param, "controller.default_yaw_vel");

  waypt_params.default_takeoff_height = bot_param_get_double_or_fail(
      bot_param, "controller.default_takeoff_height");
  waypt_params.default_takeoff_vel = bot_param_get_double_or_fail(
      bot_param, "controller.default_takeoff_vel");
  waypt_params.default_land_vel = bot_param_get_double_or_fail(
      bot_param, "controller.default_land_vel");

  //params to override navigator points if we haven't gotten where it last told us to go
  waypt_params.enforceGoalReached = bot_param_get_int_or_fail(
      bot_param, "controller.enforceGoalReached");
  //spin in place before moving
  yaw_first = bot_param_get_int_or_fail(bot_param, "controller.yaw_first");

  //params for whether to perform feedback around nonzero velocities
  useSetPointVelocity = bot_param_get_int_or_fail(
      bot_param, "controller.useSetPointVelocity");
  max_accel_xy = bot_param_get_double_or_fail(bot_param,
                                              "controller.max_accel_xy");
  max_accel_z = bot_param_get_double_or_fail(bot_param,
                                             "controller.max_accel_z");

}

static void read_parameters(int argc, char **argv, lcm_t * lcm) {
  //default params set up top where they're declared

  ConciseArgs opt(argc, argv);
  opt.add(robot_name, "r", "robot_name",
          "required: name of the robot in the config", true);
  opt.add(simulator, "s", "simulator", "Operate in simulator mode");
  opt.add(simulation_rate, "S", "simulation_rate",
          "Publish state estimates at the freq");
  opt.add(param_tune, "P", "param_tune", "Run controller in param tune mode");

  opt.parse();

  fprintf(stderr, "running with parameters for %s \n", robot_name.c_str());

  if (param_tune) {
    fprintf(stderr, "running in param tuning mode!\n");
  }

  if (simulator) {
    fprintf(stderr, "running in SIMULATOR mode!\n");
  }

  bot_param = bot_param_get_global(lcm, param_tune);
  if (bot_param == NULL) {
    fprintf(stderr,
            "ERROR: the controller requires params from the param server");
    exit(1);
  }

  use_mav_estimator = bot_param_get_boolean_or_fail(
      bot_param, "controller.use_mav_estimator");

  param_update_handler(NULL, bot_param, bot_timestamp_now(), NULL);

  if (param_tune) {
    bot_param_add_update_subscriber(bot_param, param_update_handler, NULL);
  }

}

//Initial function that is run when startup of program.
int main(int argc, char **argv) {

  lcm = bot_lcm_get_global(NULL);
  read_parameters(argc, argv, lcm);

  GMainLoop * mainloop = g_main_loop_new(NULL, FALSE);
  bot_glib_mainloop_attach_lcm(lcm);

  //initialize globals
  memset(&command_msg, 0, sizeof(command_msg));
  memset(&current_goal, 0, sizeof(current_goal));
  current_goal.xyzt[2] = 5 * LAND_RAMP_DOWN_THRESH;
  current_goal.waypt_type = QUAD_WAYPOINT_T_TYPE_LAND;
  previous_goal = current_set_point = current_goal;

  //Subscribe to the network and listens for these messages
  quad_waypoint_t_subscribe(lcm, WAYPOINT_COMMAND_CHANNEL, waypoint_handler,
                            NULL);
  quad_waypoint_t_subscribe(lcm, WAYPOINT_COMM_LOSS_CHANNEL, waypoint_handler,
                            NULL);

  mission_executor_trajectory_t_subscribe(
      lcm, EXECUTE_TRAJECTORY_CHANNEL, mission_executor_trajectory_handler,
      NULL);

  // TODO: Hacked...
  if (use_mav_estimator) {
    pose_channel = "STATE_ESTIMATOR_POSE";
    fprintf(stderr, "Using %d %s\n", use_mav_estimator, pose_channel);
    rigid_body_pose_t_subscribe(lcm, pose_channel,
                                mav_state_estimator_pose_handler, NULL);
  } else {
    pose_channel = POSE_CHANNEL;
    bot_core_pose_t_subscribe(lcm, pose_channel, pose_handler, NULL);
  }

  if (simulator) {
    g_timeout_add_full(G_PRIORITY_HIGH, (guint) 1.0 / simulation_rate * 1000,
                       simulatorPosePublish, NULL, NULL);
    quad_waypoint_t_subscribe(lcm, SIM_TELEPORT_CHANNEL, sim_teleport_handler,
                              NULL);
  }

  // run
  bot_signal_pipe_glib_quit_on_kill(mainloop);
  g_main_loop_run(mainloop);

  fprintf(stderr, "shutting down!\n");
  bot_tictoc_print_stats(BOT_TICTOC_AVG);

  return 0;
}

